package com.hitqz.robot.driver.schedule;

import cn.hutool.core.util.StrUtil;
import java.util.Objects;
import com.hitqz.robot.api.common.dto.robot.RobotGoTargetPosDto;
import com.hitqz.robot.driver.HitTrackRobotDriverImpl;
import com.hitqz.robot.driver.util.TrackCacheUtil;
import com.hitqz.robot.driver.util.TrackToolkit;
import lombok.AllArgsConstructor;
import lombok.extern.slf4j.Slf4j;
import org.springframework.scheduling.annotation.Scheduled;
import org.springframework.stereotype.Component;

@Slf4j
@Component
@AllArgsConstructor
public class TrackSchedule {

    private static boolean lastSpeedCtrl = false;

    private static boolean lastRfidStatus = false;

    private final HitTrackRobotDriverImpl robotDriver;

    /**
     * 挂轨按下去是一直下发速度的，所以没按的时候要下发 0
     */
    @Scheduled(fixedRate = 500)
    private void setRobotStop()  {
        boolean currentSpeedFlag = TrackCacheUtil.getSpeedFlag();
        if (!Objects.equals(lastSpeedCtrl, currentSpeedFlag)){
            if (!currentSpeedFlag) {
                // 停止
                TrackToolkit.setSpeed((short) 0, (short) 0);
                log.info("[track]stop move change mode -> 0,0");
                TrackToolkit.setRunningMode((byte)0);
                TrackToolkit.setControlMode((byte)0);
            }else {
                // 开始
                log.info("[track]start move change mode -> 1,1");
                TrackToolkit.setRunningMode((byte)1);
                TrackToolkit.setControlMode((byte)1);
            }
        }
        lastSpeedCtrl = currentSpeedFlag;
    }


    @Scheduled(fixedRate = 250)
    private void checkRfidNetStatus(){
        if (lastRfidStatus!=TrackToolkit.rfidReadNetError){
            if (TrackToolkit.rfidReadNetError) {
                log.info("[track] rfid通讯异常,先停原地");
                robotDriver.motionAbort();
            } else {
                if (StrUtil.isNotBlank(TrackToolkit.currentTargetCode)) {
                    log.info("[track] rfid 通讯恢复,恢复前往点位{}", TrackToolkit.currentTargetCode);
                    robotDriver.robotTaskGoTarget(RobotGoTargetPosDto.builder().code(TrackToolkit.currentTargetCode).charge(false).build());
                }
            }
        }
        lastRfidStatus = TrackToolkit.rfidReadNetError;
    }

}
